; WARRANTY OF ANY KIND, WHETHER EXPRESS OR IMPLIED, INCLUDING BUT NOT
; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
; PARTICULAR PURPOSE, OR NON-INFRINGEMENT. IN NO EVENT SHALL MICROCHIP
; BE LIABLE FOR ANY INCIDENTAL, SPECIAL, INDIRECT OR CONSEQUENTIAL
; DAMAGES, LOST PROFITS OR LOST DATA, HARM TO YOUR EQUIPMENT, COST OF
; PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
; BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF),
; ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER SIMILAR COSTS.
;
; REVISION HISTORY:
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
; Author            Date      Comments on this revision
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
; First Last Name   mm/dd/yy  Place your comments here
; Jorge Zambada     08/17/05  Assembly functions for WIB
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.include "p30f3010.inc"

	.global _SpeedCalculation
	.global _SpeedControl

;********************************************************************
; This subroutine Calculates the speed of the BLDC motor in fractional
; format using Fractional Division as follows:
;
;                                        Minimum Period
;   Actual Speed = (Fractinal Division) -----------------
;                                        Actual Period
;
;********************************************************************

_SpeedCalculation:
	MOV __MINPERIOD, W8
	MOV _Period, W9
	REPEAT #17
	DIVF W8, W9
	MOV W0, _sSpeed
	RETURN

;********************************************************************
;
;                                             ----   Proportional
;                                            |    |  Output
;                             ---------------| Kp |-----------------
;                            |               |    |                 |
;                            |                ----                  |
;Reference                   |                                     --- 
;Speed         ---           |           --------------  Integral | + | Control   -------
;     --------| + |  Error   |          |      Ki      | Output   |   | Output   |       |
;             |   |----------|----------| ------------ |----------|+  |----------| Plant |--
;        -----| - |          |          |  1 - Z^(-1)  |          |   |          |       |  |
;       |      ---           |           --------------           | + |           -------   |
;       |                    |                                     ---                      |
;       | Measured           |         -------------------  Deriv   |                       |
;       | Speed              |        |                   | Output  |                       |
;       |                     --------| Kd * (1 - Z^(-1)) |---------                        |
;       |                             |                   |                                 |
;       |                              -------------------                                  |
;       |                                                                                   |
;       |                                                                                   |
;        -----------------------------------------------------------------------------------
;
;   ControlOutput(K) = ControlOutput(K-1) 
;                    + ControlDifference(K) * (Kp + Ki + Kd)
;                    + ControlDifference(K-1) * (-Ki - 2*Kd)
;                    + ControlDifference(K-2) * Kd
;
;   Using PIDCoefficients:
;   PIDCoefficients[0] = Kp + Ki + Kd
;   PIDCoefficients[1] = -(Kp + 2*Kd)
;   PIDCoefficients[2] = Kd
;   and leting:
;   ControlOutput -> ControlOutput(K) and ControlOutput(K-1)
;   ControlDifference[0] -> ControlDifference(K)
;   ControlDifference[1] -> ControlDifference(K-1)
;   ControlDifference[2] -> ControlDifference(K-2)
;
;   ControlOutput = ControlOutput
;                 + ControlDifference[0] * PIDCoefficients[0]
;                 + ControlDifference[1] * PIDCoefficients[1]
;                 + ControlDifference[2] * PIDCoefficients[2]
;
;   This was implemented using Assembly with signed fractional and saturation enabled
;   with MAC instruction
;********************************************************************

_SpeedControl:
	BSET CORCON, #SATA ; Enable Saturation on Acc A
	; Init Pointers
	MOV #_ControlDifference, W8
	MOV #_PIDCoefficients, W10
	MOV #_sRefSpeed, W1
	MOV #_sSpeed, W2
	MOV #_ControlOutput, W0
	; Calculate most recent error with saturation, no limit checking required
	LAC [W1], A
	LAC [W2], B
	SUB A
	SAC A, [W8]
	; Prepare MAC Operands
	MOVSAC A, [W8]+=2, W4, [W10]+=2, W5
	LAC [W0], A ; Load Acc with last output
	; Perform MAC
	REPEAT #2	; Repeat 3 times
	MAC W4*W5, A, [W8]+=2, W4, [W10]+=2, W5
	; Store result in ControlOutput with saturation
	SAC A, [W0]
	BCLR CORCON, #SATA ; Disable Saturation on Acc A
	MOV #_ControlDifference, W8
	MOV [W8+2], W2 	;ControlDifference[2] = ControlDifference[1]
	MOV W2, [W8+4]
	MOV [W8], W2	;ControlDifference[1] = ControlDifference[0]
	MOV W2, [W8+2]
	RETURN;

.end
